
using System;
using System.Collections;
using System.Collections.Generic;
using IrrlichtNETCP;
using IrrlichtNETCP.Inheritable;
using IrrlichtNETCP.Extensions;
using Ode.NET;

namespace kinematix
{


		public struct PhysicsCoeffs
		{
			public float mu;
			public float cfm;
			public float erp;
			public float slip1;
			public float slip2;
		}
	
	
	public class kWorld  : ISceneNode
	{
		IntPtr world;
		IntPtr space;
		IntPtr contactgroup;
		
		IrrlichtDevice device;
		
		
		int obj_count;
		
		public bool run;
		
		const int MAX_CONTACTS = 24;		
		d.ContactGeom[] contacts = new d.ContactGeom[MAX_CONTACTS];	
		d.Contact contact;		
		
		SceneManager scene;
		
		
		public Dictionary< int, kPhysNode > nodes; 
		
		PhysicsCoeffs coeffs;
	
		public kWorld(SceneNode parent, IrrlichtDevice _device, int id) : base (parent, _device.SceneManager, id)
		{
			d.InitODE();
			world = d.WorldCreate();
			device = _device;
			
			scene = device.SceneManager;
			
			nodes = new Dictionary<int, kPhysNode>(); 
			
			coeffs = new PhysicsCoeffs();
			coeffs.mu = 0.99f;
			coeffs.cfm = 0.002f;
			coeffs.erp = 0.6f;
			coeffs.slip1 = 0.001f;
			coeffs.slip2 = 0.001f;
			
			world = d.WorldCreate();
			//space = d.HashSpaceCreate(IntPtr.Zero);
			space = d.SimpleSpaceCreate(IntPtr.Zero);
			contactgroup = d.JointGroupCreate(0);
			
            d.WorldSetGravity(world, 0, -9.81f, 0);
            
            d.WorldSetCFM(world,1e-5f);
			d.WorldSetERP(world,0.8f);

			d.WorldSetQuickStepNumIterations (world,20);
			
			//d.WorldSetAutoDisableFlag(world, 1);
			//d.WorldSetContactMaxCorrectingVel(world, 0.1f);
			//d.WorldSetContactSurfaceLayer(world, 0.001f);
            
            d.CreatePlane(space, 0, 1, 0, 0);

            
  			obj_count = 0;
			run = true;
		}
		
		public void ClearWorld()
		{
	
			Dictionary<int, kPhysNode>.Enumerator en =	nodes.GetEnumerator();
			
			while (en.MoveNext())
			{
			en.Current.Value.Destroy();
			}
			nodes.Clear();
			
			obj_count = 0;

			this.RemoveAll();

		}
		
		public void AddNode(kPhysNode node)
		{
			obj_count++;
			node.ID = obj_count;
			nodes.Add(obj_count, node);		
		}
		
		public void ApplyForces(float dt)
		{
			Dictionary<int, kPhysNode>.Enumerator en =	nodes.GetEnumerator();
			while (en.MoveNext())
			{
				en.Current.Value.ApplyForce(dt);
			}		
		}
		
		
		public bool Run
		{
			get
				{
					return run;
				}
			set
				{
					run = value;
				}
		}
		
		public void testclick(kPhysNode node, EventArgs args)
		{
			node.AddForce(new kForce(1, Vector3D.From(0,2,0)));
		}
		
		public kPhysNode AddBox(Vector3D pos)
		{
				

			kPhysNode box = new kPhysNode(this, obj_count, pos, BodyType.Box, "cube.3ds", true);
			box.Selectable = false;
			box.OnClick += testclick;
			return box;
		}
		

		public kPhysNode AddSphere()
		{
			
				kPhysNode n = new kPhysNode(this, obj_count, new Vector3D(0,10,0), BodyType.Sphere,
																				"sphere.3ds", true);
				
			    return n;
		}
		
		public kPhysNode AddCylinder()
		{
				
			kPhysNode cyl = new kPhysNode(this, obj_count, new Vector3D(0,10,0), BodyType.Cylinder, "cylinder.3ds", true);
				//kPhysNode cyl = new kPhysNode(this, obj_count, new Vector3D(0,10,0), BodyType.Cylinder, "cylinder.3ds", true);
				
				return cyl;
		}


		public kPhysNode AddCapsule()
		{
				
				kPhysNode cap = new kPhysNode(this, obj_count, new Vector3D(0,10,0), BodyType.Capsule, "capsule.3ds", true);
				
				return cap;
		}
		
		

		/*public static void dNearCallbackFunc(
							IntPtr data,
							IntPtr o1,
							IntPtr o2
							)		
		{
		IntPtr b1 = d.GeomGetBody (o1);
		IntPtr b2 = d.GeomGetBody (o2);		
		
		if (b1 != IntPtr.Zero && b2!=IntPtr.Zero && !d.AreConnected(b1,b2)) return;
		
		d.ContactGeom[] contactgeoms = new d.ContactGeom[4];
		int n;
		unsafe
		{
		n = d.Collide(o1,o2,4,contactgeoms,sizeof(d.ContactGeom));
		}
		d.Contact[] contact = new d.Contact[n];
						
		for(int i = 0; i < contact.Length; i++)
			{
				//contact[i] = new d.Contact();
				contact[i].surface.mode = 	
											(int)d.ContactFlags.Approx1 |
											//(int)d.ContactFlags.dContactSlip1 |
											//(int)d.ContactFlags.dContactSlip2 |
											(int)d.ContactFlags.SoftCFM |
											(int)d.ContactFlags.Bounce |
											(int)d.ContactFlags.SoftERP;
				//contact[i].surface.mu = coeffs.mu;
				contact[i].surface.mu = d.Infinity;
				/*contact[i].surface.slip1 = coeffs.slip1;
				contact[i].surface.slip2 = coeffs.slip2;
				contact[i].surface.bounce = 0.1f;
				contact[i].surface.bounce_vel = 0.1f;
				contact[i].surface.soft_cfm = coeffs.cfm;
				contact[i].surface.soft_erp = coeffs.erp;
				contact[i].geom = contactgeoms[i];

				IntPtr c = d.JointCreateContact(world, contactgroup, ref contact[i]);
				d.JointAttach(c,b1,b2);
			}		
		}*/
		
		void dNearCallbackFunc(IntPtr space, IntPtr g1, IntPtr g2)
		{
			IntPtr b1 = d.GeomGetBody(g1);
			IntPtr b2 = d.GeomGetBody(g2);
			IntPtr d1 = d.GeomGetData(g1);
			IntPtr d2 = d.GeomGetData(g2);
			//if (d1 != IntPtr.Zero) //this should never happen
			//{
			kPhysNode n1 = (kPhysNode)NativeElement.GetObject(d1, typeof(kPhysNode));
			kPhysNode n2 = (kPhysNode)NativeElement.GetObject(d2, typeof(kPhysNode));
			
				n1.Collide(n2);
			if (n2 != null)
			{
				//Console.WriteLine(n1.ID+" <=> "+n2.ID);
			//Console.WriteLine("hello");				
				n2.Collide(n1);
			}					
			
			//n2.Collide();
		
			//}
			bool both = (n2 != null);
			float _mu;
			if (both)
			{
				_mu = (n1.Surface.mu + n2.Surface.mu) * 0.5f;
			} else (_mu = n1.Surface.mu);
				
			
			if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact))
			{
			return;
			}

			int count = d.Collide(g1, g2, MAX_CONTACTS, contacts, d.ContactGeom.SizeOf);
			//Console.WriteLine(count);
			for (int i = 0; i < count; ++i)
			{
				contact.surface.mode = d.ContactFlags.Approx1 | d.ContactFlags.SoftCFM |
					d.ContactFlags.SoftERP | d.ContactFlags.Bounce |
						d.ContactFlags.Slip1;

				contact.surface.mu = _mu;
				contact.surface.slip1 = n1.Surface.slip1;
				//contact.surface.slip2 = coeffs.slip2;
				contact.surface.bounce = 0.1f;
				contact.surface.bounce_vel = 0.1f;
				contact.surface.soft_cfm = coeffs.cfm;
				contact.surface.soft_erp = coeffs.erp;											
				contact.geom = contacts[i];
				IntPtr joint = d.JointCreateContact(world, contactgroup, ref contact);
				d.JointAttach(joint, b1, b2);
			//if (both)
				//{
				//}
			}
		}
	
		public void Step(float step)
		{
		if (run)
		{
			d.SpaceCollide(space, IntPtr.Zero, dNearCallbackFunc);
			//d.WorldStep(world, step);
			ApplyForces(step);	
			d.WorldQuickStep(world, step);
			//d.WorldStepFast1(world, 0.001f, 20);
			d.JointGroupEmpty(contactgroup);
		}	
		}
		
		public void Destroy()
		{
			d.WorldDestroy(world);
			d.CloseODE();
		}
		
		public IntPtr World
		{
			get
				{
					return world;
				}
		}
		
		public IntPtr Space
		{
			get
				{
					return space;
				}
		}
		
		public int ObjCount
		{
			get
				{
					return obj_count;
				}
		}
		
		
		public PhysicsCoeffs Coeffs
		{
			get
				{
					return coeffs;
				}
			set
				{
					coeffs = value;
				}
		}
		
		public SceneManager SceneManager
		{
			get
			{
				return scene;
			}
		}
		
		public IrrlichtDevice Device
		{
			get{
				return device;
			}
		}
		
	}
	
}
